/*****************************************************************************************/
/*  <vn_imu_data_pub.cpp  is a program intended to use vectornav for ROS EKF package. 
 
 <If you intend to use the full services.run the vectornav.cpp program.>

 <this program provides a connection to VectorNav, This program also publishes a topic,  
 imu_data : The topic of type "sensor_msgs" to use robot_pose_ekf package>
 
    Copyright (C) <iroboApp - 2013>  
    
    <Authors: Anis Koubaa <akoubaa@coins-lab.org> , Yasir Javed <yasir.javed@coins-lab.org> 

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
/*****************************************************************************************/

/******************************** NOTES *************************************************************/
/* This cpp file produces the imu_dataPublisher node used to publish imu_data topic for ekf pose ****/
/****************************************************************************************************/


#include "vn_imu_data_pub.h" //define the interface of the class

using namespace std;
VnEkfImuDataPub::VnEkfImuDataPub(const ros::NodeHandle& nh)
  : _VNImuNodeHandle (nh)
  {
        uint64_t time=0;
	
	Vn100 vn100; // the Vn100 Handle required for connection and getting data from vectornav
	VnQuaternion quaternion;// for Quaternion. They named quaternion components with x,y,z and w in the structured defined in vn_kinmatics.h
        VnVector3 acceleration; // for accelration 
	VnVector3 angularRate; // for getting the angular rate
	
	const char* const COM_PORT = "//dev//ttyUSB0"; // the connection port of vectornav
        const int BAUD_RATE = 115200; // baudrate at which vectornav works
        
        // open VN100 serial port
        int err_val = vn100_connect(&vn100, COM_PORT, BAUD_RATE);
	
	 // if the port does not open, use the following command is terminal
        // sudo chmod 777 /dev/ttyUSB0
        // assuming that VN100 is connected to /dev/ttyUSB0 
	if  (err_val !=0){
	printf("[Error: VectorNav Constructor] Unable to open COM Port for VN-100. \n Possible Solutions:\n");
	printf("    1- Change permission of Serial Port of VN 100. \n       Try command [sudo chmod 777 /dev/ttyUSB0] if /dev/ttyUSB0 is the COM port for VN-100\n");
	printf("     2- Check the connection of VN-100 to Serial Port. \n");
	printf("     3- Check that the port number is VN-100 to Serial Port. \n");	   
	}
	else 	
        printf("[VectorNav Connection Success] COM_PORT = %s \n", COM_PORT);
	printf("[IMUDataPublisher Node] imu_data topic collected from VN100 in port %s will be published for robot_pose_ekf node  \n", COM_PORT);
	
       
       /******** Delcare Imu Data publisher ****************/
       
  	_vectornavIMUDataPublisher = _VNImuNodeHandle.advertise<sensor_msgs::Imu>("imu_data", 10);
	//create a message to be published for angular rate
	sensor_msgs::Imu vn100_Imu_data_topic_object;

	/******** Start and loop publishing ****************/
	
	while (ros::ok()){

	  vn100_getQuaternion(&vn100, &quaternion);
	  vn100_getAcceleration(&vn100, &acceleration);
	  vn100_getAngularRate(&vn100, &angularRate);
	  
	  /************************************************************************************************/
	  /************* This part is for updating IMU data needed for robot_pose_ekf *********************/
	  /************************************************************************************************/
	   // for publishing the IMU data
	  // derived from http://www.ros.org/doc/api/microstrain_3dmgx2_imu/html/imu__node_8cc_source.html
	   
	  
	    //get acceleration values from vn-100
	    vn100_Imu_data_topic_object.linear_acceleration.x = acceleration.c0;
	    vn100_Imu_data_topic_object.linear_acceleration.y = acceleration.c1;
	    vn100_Imu_data_topic_object.linear_acceleration.z = acceleration.c2;
	  
	    //get angular rates values from vn-100
	    vn100_Imu_data_topic_object.angular_velocity.x = angularRate.c0;
	    vn100_Imu_data_topic_object.angular_velocity.y = angularRate.c1;
	    vn100_Imu_data_topic_object.angular_velocity.z = angularRate.c2;
	        
	    //get quaternion values from vn-100
	    vn100_Imu_data_topic_object.orientation.x = quaternion.x;
	    vn100_Imu_data_topic_object.orientation.y = quaternion.y;
	    vn100_Imu_data_topic_object.orientation.z = quaternion.z;
	    vn100_Imu_data_topic_object.orientation.w = quaternion.w;
	    
	    // setting orientation covariance. values are taken from example of microstrain_3dmgx2_imu
	    vn100_Imu_data_topic_object.orientation_covariance[0] = -1;
	    vn100_Imu_data_topic_object.orientation_covariance[1] = 0;
	    vn100_Imu_data_topic_object.orientation_covariance[2] = 0;
	    vn100_Imu_data_topic_object.orientation_covariance[3] = 0;
	    vn100_Imu_data_topic_object.orientation_covariance[4] = 1;
	    vn100_Imu_data_topic_object.orientation_covariance[5] = 0;
	    vn100_Imu_data_topic_object.orientation_covariance[6] = 0;
	    vn100_Imu_data_topic_object.orientation_covariance[7] = 0;
	    vn100_Imu_data_topic_object.orientation_covariance[8] = -1;
	    
	    //get current time
	    vn100_Imu_data_topic_object.header.stamp = ros::Time::now().fromNSec(time);
	    //publish the imu__node topic for robot_pose_ekf node 
	    _vectornavIMUDataPublisher.publish(vn100_Imu_data_topic_object);
	   
	}
	//if leaves the endless loop, disconnect from vn-100
	vn100_disconnect(&vn100);

}

/* Destructor */
VnEkfImuDataPub::~VnEkfImuDataPub()
{
  ROS_INFO("[VnEkfImuDataPub::~VnEkfImuDataPub()] Destructor Called! ");
}

// Main Function
int main (int argc, char **argv)
{
  ros::init(argc, argv, "VNImubase_footprint");
  ROS_INFO("Welcome to VnEkfImuDataPub ROS Driver");
  VnEkfImuDataPub _self(ros::NodeHandle("")); 
  
  return 0;
}
